﻿/*
* Farseer Physics Engine based on Box2D.XNA port:
* Copyright (c) 2010 Ian Qvist
* 
* Box2D.XNA port of Box2D:
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
*
* Original source Box2D:
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
* 
* This software is provided 'as-is', without any express or implied 
* warranty.  In no event will the authors be held liable for any damages 
* arising from the use of this software. 
* Permission is granted to anyone to use this software for any purpose, 
* including commercial applications, and to alter it and redistribute it 
* freely, subject to the following restrictions: 
* 1. The origin of this software must not be misrepresented; you must not 
* claim that you wrote the original software. If you use this software 
* in a product, an acknowledgment in the product documentation would be 
* appreciated but is not required. 
* 2. Altered source versions must be plainly marked as such, and must not be 
* misrepresented as being the original software. 
* 3. This notice may not be removed or altered from any source distribution. 
*/

using System.Collections.Generic;
using FarseerPhysics.Collision;
using FarseerPhysics.Dynamics.Contacts;

namespace FarseerPhysics.Dynamics
{
	public class ContactManager
	{
		/// <summary>
		/// Fires when a contact is created
		/// </summary>
		public BeginContactDelegate BeginContact;

		public IBroadPhase BroadPhase;

		/// <summary>
		/// The filter used by the contact manager.
		/// </summary>
		public CollisionFilterDelegate ContactFilter;

		public List<Contact> ContactList = new List<Contact>(128);

		/// <summary>
		/// Fires when a contact is deleted
		/// </summary>
		public EndContactDelegate EndContact;

		/// <summary>
		/// Fires when the broadphase detects that two Fixtures are close to each other.
		/// </summary>
		public BroadphaseDelegate OnBroadphaseCollision;

		/// <summary>
		/// Fires after the solver has run
		/// </summary>
		public PostSolveDelegate PostSolve;

		/// <summary>
		/// Fires before the solver runs
		/// </summary>
		public PreSolveDelegate PreSolve;

		internal ContactManager(IBroadPhase broadPhase)
		{
			this.BroadPhase = broadPhase;
			this.OnBroadphaseCollision = this.AddPair;
		}

		// Broad-phase callback.
		private void AddPair(ref FixtureProxy proxyA, ref FixtureProxy proxyB)
		{
			Fixture fixtureA = proxyA.Fixture;
			Fixture fixtureB = proxyB.Fixture;

			int indexA = proxyA.ChildIndex;
			int indexB = proxyB.ChildIndex;

			Body bodyA = fixtureA.Body;
			Body bodyB = fixtureB.Body;

			// Are the fixtures on the same body?
			if (bodyA == bodyB)
			{
				return;
			}

			// Does a contact already exist?
			ContactEdge edge = bodyB.ContactList;
			while (edge != null)
			{
				if (edge.Other == bodyA)
				{
					Fixture fA = edge.Contact.FixtureA;
					Fixture fB = edge.Contact.FixtureB;
					int iA = edge.Contact.ChildIndexA;
					int iB = edge.Contact.ChildIndexB;

					if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
					{
						// A contact already exists.
						return;
					}

					if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
					{
						// A contact already exists.
						return;
					}
				}

				edge = edge.Next;
			}

			// Does a joint override collision? Is at least one body dynamic?
			if (bodyB.ShouldCollide(bodyA) == false)
				return;

			//Check default filter
			if (ShouldCollide(fixtureA, fixtureB) == false)
				return;

			// Check user filtering.
			if (this.ContactFilter != null && this.ContactFilter(fixtureA, fixtureB) == false)
				return;

			if (!fixtureA.Body.OnBeforeCollision(fixtureA, fixtureB)) return;
			if (!fixtureB.Body.OnBeforeCollision(fixtureB, fixtureA)) return;

			// Call the factory.
			Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);

			// Contact creation may swap fixtures.
			fixtureA = c.FixtureA;
			fixtureB = c.FixtureB;
			bodyA = fixtureA.Body;
			bodyB = fixtureB.Body;

			// Insert into the world.
			this.ContactList.Add(c);

			// Connect to island graph.

			// Connect to body A
			c.NodeA.Contact = c;
			c.NodeA.Other = bodyB;

			c.NodeA.Prev = null;
			c.NodeA.Next = bodyA.ContactList;
			if (bodyA.ContactList != null)
			{
				bodyA.ContactList.Prev = c.NodeA;
			}
			bodyA.ContactList = c.NodeA;

			// Connect to body B
			c.NodeB.Contact = c;
			c.NodeB.Other = bodyA;

			c.NodeB.Prev = null;
			c.NodeB.Next = bodyB.ContactList;
			if (bodyB.ContactList != null)
			{
				bodyB.ContactList.Prev = c.NodeB;
			}
			bodyB.ContactList = c.NodeB;
		}

		internal void FindNewContacts()
		{
			this.BroadPhase.UpdatePairs(this.OnBroadphaseCollision);
		}

		internal void Destroy(Contact contact)
		{
			Fixture fixtureA = contact.FixtureA;
			Fixture fixtureB = contact.FixtureB;
			Body bodyA = fixtureA.Body;
			Body bodyB = fixtureB.Body;

			if (contact.IsTouching())
			{
				//Report the separation to both participants:
				if (bodyA != null) bodyA.OnSeparation(fixtureA, fixtureB);

				//Reverse the order of the reported fixtures. The first fixture is always the one that the
				//user subscribed to.
				if (bodyB != null) bodyB.OnSeparation(fixtureB, fixtureA);

				if (this.EndContact != null) this.EndContact(contact);
			}

			// Remove from the world.
			this.ContactList.Remove(contact);

			// Remove from body 1
			if (contact.NodeA.Prev != null)
			{
				contact.NodeA.Prev.Next = contact.NodeA.Next;
			}

			if (contact.NodeA.Next != null)
			{
				contact.NodeA.Next.Prev = contact.NodeA.Prev;
			}

			if (contact.NodeA == bodyA.ContactList)
			{
				bodyA.ContactList = contact.NodeA.Next;
			}

			// Remove from body 2
			if (contact.NodeB.Prev != null)
			{
				contact.NodeB.Prev.Next = contact.NodeB.Next;
			}

			if (contact.NodeB.Next != null)
			{
				contact.NodeB.Next.Prev = contact.NodeB.Prev;
			}

			if (contact.NodeB == bodyB.ContactList)
			{
				bodyB.ContactList = contact.NodeB.Next;
			}

			contact.Destroy();
		}

		internal void Collide()
		{
			// Update awake contacts.
			for (int i = 0; i < this.ContactList.Count; i++)
			{
				Contact c = this.ContactList[i];
				Fixture fixtureA = c.FixtureA;
				Fixture fixtureB = c.FixtureB;
				int indexA = c.ChildIndexA;
				int indexB = c.ChildIndexB;
				Body bodyA = fixtureA.Body;
				Body bodyB = fixtureB.Body;

				if (bodyA.Awake == false && bodyB.Awake == false)
				{
					continue;
				}

				// Is this contact flagged for filtering?
				if ((c.Flags & ContactFlags.Filter) == ContactFlags.Filter)
				{
					// Should these bodies collide?
					if (bodyB.ShouldCollide(bodyA) == false)
					{
						Contact cNuke = c;
						Destroy(cNuke);
						continue;
					}

					// Check default filtering
					if (ShouldCollide(fixtureA, fixtureB) == false)
					{
						Contact cNuke = c;
						Destroy(cNuke);
						continue;
					}

					// Check user filtering.
					if (this.ContactFilter != null && this.ContactFilter(fixtureA, fixtureB) == false)
					{
						Contact cNuke = c;
						Destroy(cNuke);
						continue;
					}

					// Clear the filtering flag.
					c.Flags &= ~ContactFlags.Filter;
				}

				int proxyIdA = fixtureA.Proxies[indexA].ProxyId;
				int proxyIdB = fixtureB.Proxies[indexB].ProxyId;

				bool overlap = this.BroadPhase.TestOverlap(proxyIdA, proxyIdB);

				// Here we destroy contacts that cease to overlap in the broad-phase.
				if (overlap == false)
				{
					Contact cNuke = c;
					Destroy(cNuke);
					continue;
				}

				// The contact persists.
				c.Update(this);
			}
		}

		private static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB)
		{
			if (Settings.UseFPECollisionCategories)
			{
				if ((fixtureA.CollisionGroup == fixtureB.CollisionGroup) &&
					fixtureA.CollisionGroup != 0 && fixtureB.CollisionGroup != 0)
					return false;

				if (((fixtureA.CollisionCategories & fixtureB.CollidesWith) ==
					 Category.None) &
					((fixtureB.CollisionCategories & fixtureA.CollidesWith) ==
					 Category.None))
					return false;

				if (fixtureA.IsFixtureIgnored(fixtureB) ||
					fixtureB.IsFixtureIgnored(fixtureA))
					return false;

				return true;
			}

			if (fixtureA.CollisionGroup == fixtureB.CollisionGroup &&
				fixtureA.CollisionGroup != 0)
			{
				return fixtureA.CollisionGroup > 0;
			}

			bool collide = (fixtureA.CollidesWith & fixtureB.CollisionCategories) != 0 &&
						   (fixtureA.CollisionCategories & fixtureB.CollidesWith) != 0;

			if (collide)
			{
				if (fixtureA.IsFixtureIgnored(fixtureB) ||
					fixtureB.IsFixtureIgnored(fixtureA))
				{
					return false;
				}
			}

			return collide;
		}
	}
}